/*********************************************************************************************************************
 * COPYRIGHT NOTICE
 * Copyright (c) 2020,��ɿƼ�
 * All rights reserved.
 * ��������QQȺ��һȺ��179029047(����)  ��Ⱥ��244861897(����)  ��Ⱥ��824575535
 *
 * �����������ݰ�Ȩ������ɿƼ����У�δ����������������ҵ��;��
 * ��ӭ��λʹ�ò������������޸�����ʱ���뱣����ɿƼ��İ�Ȩ������
 *
 * @file       		main
 * @company	   		�ɶ���ɿƼ����޹�˾
 * @author     		��ɿƼ�(QQ790875685)
 * @version    		�鿴doc��version�ļ� �汾˵��
 * @Software 		MDK FOR C251 V5.60
 * @Target core		STC32G12K128
 * @Taobao   		https://seekfree.taobao.com/
 * @date       		2020-12-18
 ********************************************************************************************************************/

#include "headfile.h"
#include "math.h"
/*
 *�����ں�Ƶ�ʵ��趨�����Բ鿴board.h�ļ�
 *��board_init��,�Ѿ���P54��������Ϊ��λ
 *�����Ҫʹ��P54����,������board.c�ļ��е�board_init()������ɾ��SET_P54_RESRT����
 
1.timer��
		TIM_0,��ͨ��ʱ�� 10ms  
		TIM_1,���ֱ�����
		TIM_2,����uart��115200��   	
		TIM_3,��ͨ��ʱ�� 50ms
		TIM_4,���ֱ�����
2.uart
	  UART_1,no init
    UART_2,no init
    UART_3,no init
    UART_4,(hc-05)
 */
int dx = 0; //v831上可以加一下近似处理
int dy = 0;
int last_dx = 0;
int last_dy = 0;
double yaw_p = 0.1;
double yaw_d = 0.0;
double pitch_p = 0.1;
double pitch_d = 0.0;
double task4_yaw = 909.0; //运动控制yaw,需要定义初始值来给出第一个点要打的位置
double task4_pitch = 382.0;//运动控制pitch
void apply_point(void);
void get_xy_points(void);
void main()
{
	WTST = 0;              
	DisableGlobalIRQ();		
	sys_clk = 35000000;     
	board_init();			
	
	Board_Common_Init();
	R(0);G(1);B(0);
	EnableGlobalIRQ();
  while(1)
	{			
		get_xy_points(); //当发生dx，dy给值时，这个函数就会开始追踪偏差
		Check_and_Change_Desk();
		if(_now_mod==_desk4&&KEY1_IN){apply_point();R(0);G(0);B(1);}
		if(flag_task==2)
		{
			cloud_yaw_duty=zuobiao[1][0];
			cloud_pitch_duty=zuobiao[1][1];
			Steer_Y_P(cloud_yaw_duty,cloud_pitch_duty);
			delay_ms(1000);
			
			cloud_yaw_duty=zuobiao[2][0];
			cloud_pitch_duty=zuobiao[2][1];
			Steer_Y_P(cloud_yaw_duty,cloud_pitch_duty);
			delay_ms(1000);
			cloud_yaw_duty=zuobiao[3][0];
			cloud_pitch_duty=zuobiao[3][1];
			Steer_Y_P(cloud_yaw_duty,cloud_pitch_duty);
			delay_ms(1000);
			cloud_yaw_duty=zuobiao[4][0];
			cloud_pitch_duty=zuobiao[4][1];
			Steer_Y_P(cloud_yaw_duty,cloud_pitch_duty);
			delay_ms(1000);
			cloud_yaw_duty=zuobiao[1][0];
			cloud_pitch_duty=zuobiao[1][1];
			Steer_Y_P(cloud_yaw_duty,cloud_pitch_duty);
			delay_ms(1000);
			flag_task=0;
		}
		if(flag_task==3)
		{
			Move_Peihe();
		}
		if(flag_task == 4)
		{
			cloud_yaw_duty=task3[1][0];
			cloud_pitch_duty=task3[1][1];
			Steer_Y_P(cloud_yaw_duty,cloud_pitch_duty);
			delay_ms(1000);
			
			cloud_yaw_duty=task3[2][0];
			cloud_pitch_duty=task3[2][1];
			Steer_Y_P(cloud_yaw_duty,cloud_pitch_duty);
			delay_ms(1000);
			cloud_yaw_duty=task3[3][0];
			cloud_pitch_duty=task3[3][1];
			Steer_Y_P(cloud_yaw_duty,cloud_pitch_duty);
			delay_ms(1000);
			cloud_yaw_duty=task3[4][0];
			cloud_pitch_duty=task3[4][1];
			Steer_Y_P(cloud_yaw_duty,cloud_pitch_duty);
			delay_ms(1000);
			cloud_yaw_duty=task3[1][0];
			cloud_pitch_duty=task3[1][1];
			Steer_Y_P(cloud_yaw_duty,cloud_pitch_duty);
			delay_ms(1000);
			flag_task=0;
		}
		if(flag_task == 6)
		{
			get_xy_points();
		}
		if(flag_task == 5)
		{
			get_xy_points(); //当发生dx，dy给值时，这个函数就会开始追踪偏差
			if(dx<-1)
			{
				renwusanj8_yaw-=0.2;
			}
			else if(dx>1)
			{
				renwusanj8_yaw+=0.2;
			}
			else
			renwusanj8_yaw=renwusanj8_yaw;
			
			if(dy<-1)
			{
				renwusanj8_pitch-=0.2;
			}
			else if(dy>1)
			{
				renwusanj8_pitch+=0.2;
			}
			else
				renwusanj8_pitch=renwusanj8_pitch;
			
			Steer_Y_P((int)renwusanj8_yaw,(int)renwusanj8_pitch);
			delay_ms(50);

		}
		
		
	}
}

//传入参数1：上一个设置的偏航角Yaw(x)的pwm值地址（注意此函数更新不要太快）
//传入参数2：上一个设置的俯仰角pitch(y)的pwm值地址（注意此函数更新不要太快）
//传入参数3：分度系数t,时间单位,作用可以直接改小整体运动效果
//隐形参数4：最近一次读取的红点偏差dx，直接被更新在串口处理中，所以此函数放在get_xy_points之后
//隐形参数5：最近一次读取的红点偏差dy
//隐形参数6：上上次读取的红点偏差last_dx，直接被更新在串口处理中，所以此函数放在get_xy_points之后
//隐形参数7：上上次读取的红点偏差last_dy
//pid参数6： yaw_p,yaw_d
//pid参数7： pitch_p,pitch_d 
void wml(double* yaw_duty,double *pitch_duty,unsigned int t) //控制周期可以考虑放在每次串口更新之后
{
//	if( abs(last_dx - dx) + abs(last_dy - dy) >= 80 )
//		{//如果发生目标点位跳变时，不进行微分计算，这个函数可能没必要，并且正常使用前提是舵机已经不会突变了
//		last_dx = dx;
//		last_dy = dy;
//	}
	
//	double add_yaw = (yaw_p*dx/t + yaw_d*(last_dx-dx)/t);
//	double add_pitch = (pitch_p*dy/t + pitch_d*(last_dy-dy)/t);
	double add_yaw = 0;
	double add_pitch = 0;
	signed char tempy=1,tempp=1;
	
	if(abs(dx) < 4)dx = 0;
	if(abs(dy) < 4)dy = 0;
	add_yaw = (yaw_p*dx/t + yaw_d*(last_dx-dx)/t);
	add_pitch = (pitch_p*dy/t + pitch_d*(last_dy-dy)/t);
	
	if(add_yaw<0)tempy = -1;
	if(add_pitch<0)tempp = -1;
	
	//限幅
	add_yaw = add_yaw < 1.4?(add_yaw>0.01?add_yaw:0.1):1.4;
	add_pitch = add_pitch < 1.4?(add_pitch>0.1?add_pitch:0.1):1.4;
	//限幅
	add_yaw = add_yaw < 1.4?add_yaw:1.4;
	add_pitch = add_pitch < 1.4?add_pitch:1.4;

	*yaw_duty += 1 * tempy * add_yaw; //常数1，可以改为-1，具体和dx与duty实际变化方向有关
	*pitch_duty += 1 * tempp * add_pitch;
	//限幅
	*yaw_duty = (*yaw_duty) < 952?((*yaw_duty)>866?(*yaw_duty):866):952;
	*pitch_duty = (*pitch_duty) < 422?((*pitch_duty)>341?(*pitch_duty):341):422;

	last_dx = dx;
	last_dy = dy;
}



void get_xy_points(void)
{
	if(jieshou_flag == 1)
	{
		unsigned int i = 1;
		char a = jieshoushuzu[1];
		int temp = 0;
		R(0);G(0);B(0);
		while(a != '@')
	  {
			if(a == ',')
			{
				dx = temp;
				temp = 0;
			}
			else
			{
				temp = temp*10 + a - 48;
			}
			i++;
			a = jieshoushuzu[i];
		}
		dy = temp;
		jieshou_count = 0;
		jieshou_flag = 0;
		
		dx = dx - 300;
		dy = dy - 300;
//		wml(&task4_yaw,&task4_pitch,1); 
//		Steer_Y_P((int)(task4_yaw),(int)(task4_pitch));
	}
}

//void get_rect_points(void)
//{
//	if(jieshou_flag == 1)
//	{
//		unsigned int i = 1,j = 0,k = 0;
//		char a = jieshoushuzu[1];
//		int temp = 0;
//		R(0);G(0);B(0);
//		while(a != '@')
//	  {
//			if(a == ',')
//			{
//				if((k%2) == 1)
//				{
//					rect_point[j][1] = temp;
//					j++;
//				}
//				else
//				{
//					rect_point[j][0] = temp;
//				}
//				k++;
//				temp = 0;
//			}
//			else
//			{
//				temp = temp*10 + a - 48;
//			}
//			i++;
//			a = jieshoushuzu[i];
//		}
//		rect_point[3][1] = temp;
//		jieshou_count = 0;
//		jieshou_flag = 0;
//	}
//}



